#include <core/context.hpp>

const static std::map<Context::NodeName, std::string> node_name_map = {
    {Context::NodeName::PathController, "follow_controller"},
    {Context::NodeName::StateController, "state_controller"},
    {Context::NodeName::LocalPlaner, "local_planer"}};
Context::Context(std::weak_ptr<rclcpp::Node> node, std::string prefix)
    : node(node)
{
    auto create_set =
        [prefix](NodeName name, std::shared_ptr<rclcpp::Node> node)
    {
        std::string service_name =
            prefix + '/' + node_name_map.at(name) + "/change_state";
        auto client = node->create_client<ChangeState>(service_name);
        for (int i = 0; i < 10; i++)
        {
            if (!client->wait_for_service(std::chrono::seconds(1)))
            {
                RCLCPP_ERROR(node->get_logger(),
                             "Service set:[%s]not available",
                             service_name.c_str());
            }
            else
            {
                break;
            }
        }
        if (!client->wait_for_service(std::chrono::seconds(1)))
        {
            return rclcpp::Client<ChangeState>::SharedPtr();
        }
        return client;
    };
    auto create_get =
        [prefix](NodeName name, std::shared_ptr<rclcpp::Node> node)
    {
        std::string service_name =
            prefix + '/' + node_name_map.at(name) + "/get_state";
        auto client = node->create_client<GetState>(service_name);
        for (int i = 0; i < 10; i++)
        {
            if (!client->wait_for_service(std::chrono::seconds(1)))
            {
                RCLCPP_ERROR(node->get_logger(),
                             "Service get:[%s]not available",
                             service_name.c_str());
            }
            else
            {
                break;
            }
        }
        if (!client->wait_for_service(std::chrono::seconds(1)))
        {
            return rclcpp::Client<GetState>::SharedPtr();
        }
        return client;
    };
    auto nodePtr = node.lock();
    if (!nodePtr)
        throw std::runtime_error("Failed to get node");
    for (auto [name, str] : node_name_map)
    {
        auto client_set = create_set(name, nodePtr);
        if (client_set)
        {
            set_clients[name] = client_set;
            is_active[name] = false;
            RCLCPP_INFO(nodePtr->get_logger(), "Service :[%s] for set ready",
                        str.c_str());
        }
        else
            throw std::runtime_error("Failed to create client");
        auto client_get = create_get(name, nodePtr);
        if (client_get)
        {
            get_clients[name] = client_get;
            RCLCPP_INFO(nodePtr->get_logger(), "Service :[%s] for get ready",
                        str.c_str());
        }
        else
            throw std::runtime_error("Failed to create client");
    }
}
void Context::Initialize()
{
    for (auto [name, client] : set_clients)
    {
        ChangeNodeState(name,
                        lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
    }
}

void Context::Finalize()
{

    for (auto [name, client] : set_clients)
    {
        ChangeNodeState(name,
                        lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP);
    }
}

void Context::ChangeNodeState(NodeName node, uint8_t state)
{
    auto request = std::make_shared<ChangeState::Request>();
    request->transition.id = state;
    auto future = set_clients[node]->async_send_request(request);
}
void Context::ActiveNode(NodeName node)
{
    if (IsActive(node))
        return;
    ChangeNodeState(node, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
    is_active[node] = true;
}

void Context::DeactiveNode(NodeName node)
{
    if (!IsActive(node))
        return;
    ChangeNodeState(node,
                    lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
    is_active[node] = false;
}

bool Context::IsActive(NodeName node)
{
    return is_active[node];
    // if (!get_clients[node]->wait_for_service(std::chrono::seconds(1)))
    //{
    //     RCLCPP_ERROR(rclcpp::get_logger("core/context"),
    //                  "Service :[%s]not available",
    //                  node_name_map.at(node).c_str());
    // }
    // auto request = std::make_shared<GetState::Request>();
    // auto future = get_clients[node]->async_send_request(request);
    // auto core = this->node.lock();
    // if (!core)
    //{
    //     RCLCPP_ERROR(rclcpp::get_logger("core/context"), "node is nullptr");
    // }
    // if (rclcpp::spin_until_future_complete(core, future) ==
    //     rclcpp::FutureReturnCode::SUCCESS)
    //{
    //     auto response = future.get();
    //     return response->current_state.id ==
    //            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
    // }
    // throw std::runtime_error("Failed to get state");
}

std::shared_ptr<rclcpp::Node> Context::GetNode()
{
    return node.lock();
}